///////////////////////////////
// servos mechanism
// 0.1

int init_servos()
{
  for(int i=0; i<N_SERVOS; i++)
  {
    if(servo_pin[i] != -1)
      servos[i].attach(servo_pin[i]);
  }
}

void update_servos()
{
  for(int i=0; i<N_SERVOS; i++)
  {
    if(servo_update[i])
    {
      servos[i].write(servo_pos[i]+servo_cal[i]);
      servo_update[i] = false;
    }
  }
}

int set_servo_position(int iInd, int iPos)
{
  for(int i=0; i<N_SERVOS; i++)
  {
    if(servo_ind[i] == iInd)
    {
      int p = iPos - servo_pos[i];
      servo_pos[i] = iPos;
      servo_update[i] = true;
      return (p<0?-p:p);
    }
  }
  Serial.print("cannot find servo '");
  Serial.print(iInd);
  Serial.println("' !");
  return -1;
}

